#pragma once
#include "cartographer/rcm/dynamic/dynamic_detect.h"

namespace cartographer
{
    struct DynCell
    {
        int hitCnt;
        int uniqueHitCnt;
        float probability;
    };

    class DynGridMap
    {
    public:
        using Ptr = std::shared_ptr<DynGridMap>;

        DynGridMap(const mapping::MapLimits &limitOrg, float pixelScale, float hitRatio);

        inline bool HasCell(int index)
        {
            return cells.find(index) != cells.end();
        }

        inline const DynCell &GetCell(int index)
        {
            return cells[index];
        }

        inline float GetProbability(int index)
        {
            return cells[index].probability;
        }

        int GetLinIndex(const Eigen::Vector2f &point);

        void Update(const Eigen::Vector2f &point, bool uniqueHit);

        float hitRatio;
        float resolution;
        std::map<int, DynCell> cells;

        mapping::MapLimits limit;
    };

    class MultiDynamicDetector : public DynamicDetector
    {
    public:
        using Ptr = std::shared_ptr<MultiDynamicDetector>;

        MultiDynamicDetector();

        void DetectDynPoint(mapping::Grid2D *grid, const sensor::RangeData &range_data, const common::Time time, std::vector<int> &dynCnts);

        void DetectDynPoint2(mapping::Grid2D *grid, const sensor::RangeData &range_data, const common::Time time,
                             std::vector<int> &dynCnts);

        void PickDynPoints(std::vector<int> &lastDynCnts, std::vector<int> &dynCnts, const sensor::RangeData &range_data);

        void PickDynPoints(std::vector<int> &lastDynCnts, std::vector<int> &dynCnts, const sensor::RangeData &range_data,
                           std::vector<Eigen::Vector2f> &dynPoints);

    private:
        std::vector<float> scales_;
        std::vector<float> hitRatios_;
        std::deque<sensor::RangeData> cloudQueue_;
    };
} // namespace cartographer